#include "pid.h"
#include "calculate.h"

float kp = 2.0f;  // 比例增益
float ki = 1.0f;  // 积分增益
float kd = 1.0f;  // 微分增益

//pid参数初始化
void pid_init(pid_t *pid)
{
	pid->kp=kp;
	pid->ki=ki;
  pid->kd=kd;
}

//速度环
void pid_speed(pid_t *pid,float target,float current)
{
  pid->target_value = target;
	pid->feedback_value = current;
	pid->err[1] = pid->err[0]; //上次误差
	pid->err[0] = pid->target_value - pid->feedback_value; //这次误差
	
	pid->p_out = pid->kp * pid->err[0]; //比例
	pid->i_out += pid->ki * pid->err[0]; //积分
	pid->d_out = pid->kd * (pid->err[0]-pid->err[1]);  //微分
	pid->out = pid->p_out + pid->i_out + pid->d_out;
	
  Limit_Min_Max(pid->out, -25000,25000); //输出限幅
	
}
